#include "string.h"
#include "Math.h"
#include "windows.h"
#include<stdio.h>
#include<conio.h>

#include "Com.h"
#include "wit_c_sdk.h"
#include "WitSensor.h"

static void DelayMs(uint16_t ms)
{
	Sleep(ms);
}

static void SensorUartSend(uint8_t* p_data, uint32_t uiSize)
{
	SendUARTMessageLength((const char*)p_data, uiSize);
}
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
{
	s_cDataUpdate = 1;
}

static bool AutoScanSensor(void)
{
	const uint32_t c_uiBaud[7] = { 4800, 9600, 19200, 38400, 57600, 115200, 230400 };
	int i, iRetry;

	for (i = 0; i < 7; i++)
	{
		CloseCOMDevice();
		OpenCOMDevice(iComPort, c_uiBaud[i]);
		//SetBaundrate(c_uiBaud[i]);
		iRetry = 2;
		do
		{
			s_cDataUpdate = 0;
			WitReadReg(AX, 3);
			//Sleep(100);
			if (s_cDataUpdate != 0)
			{
				printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
				return true;
			}
			iRetry--;
		} while (iRetry);
	}

	return false;
}

WitSensor::~WitSensor()
{
	CloseCOMDevice();
}

bool WitSensor::Init()
{
	for (iComPort = 0; iComPort < 10; iComPort++)
	{
		OpenCOMDevice(iComPort, iBaud);
		WitInit(WIT_PROTOCOL_NORMAL, 0x50);
		WitSerialWriteRegister(SensorUartSend);
		WitRegisterCallBack(CopeSensorData);

		bool init = AutoScanSensor();
		if (init)
		{
			return true;
		}
	}
	printf("can not find sensor\r\n");
	printf("please check your connection\r\n");
	return false;
}

void WitSensor::CollectData()
{
	for (int i = 0; i < 3; i++)
	{
		a[i] = (float)sReg[AX + i] / 32768.0f * 16.0f;
		w[i] = (float)sReg[GX + i] / 32768.0f * 2000.0f;
		Angle[i] = (float)sReg[Roll + i] / 32768.0f * 180.0f;
		h[i] = (float)sReg[HX + i];
	}
}

void WitSensor::GetAcceleration(float& x, float& y, float& z)
{
	x = a[0];
	y = a[1];
	z = a[2];
}

void WitSensor::GetAngle(float& x, float& y, float& z)
{
	x = Angle[0];
	y = Angle[1];
	z = Angle[2];
}